#include <iostream>
#include <fstream>
#include <iomanip>
#include "Mechanization.hpp"
#include "utils.hpp"

int main(int argc, char** argv){
    std::ifstream data_input_fs("/home/ghowoght/workspace/IntegratedNavigation/data/IMU.bin", std::ios::binary);
    Nav::ImuData data_raw;
    std::ifstream result_ref_fs("/home/ghowoght/workspace/IntegratedNavigation/data/Reference.bin", std::ios::binary);
    Nav::result_st result_ref;
    do{
        data_input_fs.read((char*)(&data_raw), sizeof(data_raw));
    }while(data_raw.timestamp < Nav::START_TIME);

    auto state = std::make_shared<Nav::State>();
    state->timestamp = data_raw.timestamp;
    state->pos << Nav::D2R * 23.1373950708, Nav::D2R * 113.3713651222, 2.175;
    state->vel << 0, 0, 0;
    state->att << Nav::D2R * -75.7498049314083, Nav::D2R * -2.14251290749072, Nav::D2R * 0.0107951084511778; // yaw pit rol
    Nav::Mechanization mec(state, data_raw);

    while(!data_input_fs.eof()){
    // for(int i = 0; i < 10000; i++){
        data_input_fs.read((char*)(&data_raw), sizeof(data_raw));
        mec.update(data_raw);

        if(1){
            // 打印结果
            result_ref_fs.read((char*)(&result_ref), sizeof(result_ref));
            std::cout.precision(12);
            std::cout << "--ref    : ";
            // std::cout   << result_ref.pos[0] << " " 
            //             << result_ref.pos[1] << " " 
            //             << result_ref.pos[2] << std::endl;

            std::cout   << result_ref.att[0] << " " 
                        << result_ref.att[1] << " " 
                        << result_ref.att[2] << std::endl;
            // std::cout   << result_ref.vel[0] << " " 
            //             << result_ref.vel[1] << " " 
            //             << result_ref.vel[2] << std::endl;

            std::cout << "--compute: ";
            // std::cout   << Nav::R2D * state->pos[Nav::LAT] << " " 
            //             << Nav::R2D * state->pos[Nav::LON] << " " 
            //             << state->pos[Nav::ALT] << std::endl;
            // std::cout   << state_->vel[N] << " " 
            //             << state_->vel[E] << " " 
            //             << state_->vel[G] << std::endl;
            std::cout   << Nav::R2D * state->att[Nav::ROL] << " " 
                        << Nav::R2D * state->att[Nav::PIT] << " " 
                        << Nav::R2D * state->att[Nav::YAW] << std::endl;
            std::cout << "-----------" << std::endl;
        }

    }
    data_input_fs.close();

    // Eigen::Quaterniond qatt;
    // qatt    = Eigen::AngleAxisd(ori_state.att[Nav::YAW], Eigen::Vector3d::UnitZ())
    //         * Eigen::AngleAxisd(ori_state.att[Nav::PIT], Eigen::Vector3d::UnitY())
    //         * Eigen::AngleAxisd(ori_state.att[Nav::ROL], Eigen::Vector3d::UnitX());
    // qatt.normalize();
    
    // Eigen::Vector3d att1 = qatt.toRotationMatrix().eulerAngles(2, 1, 0); 
    // std::cout << ori_state.att.transpose() * 180.0 / Nav::PI << std::endl;
    // std::cout << att1.transpose() * 180.0 / Nav::PI << std::endl;

    // Eigen::Matrix3d dcm = qatt.toRotationMatrix();
    // Eigen::Vector3d att2;
    // Nav::DCM2Euler(dcm, att2);
    // std::cout << att2.transpose() * 180.0 / Nav::PI << std::endl;

    // Nav::Quat2DCM(qatt, dcm);
    // Eigen::Vector3d att3;
    // Nav::DCM2Euler(dcm, att3);
    // std::cout << att3.transpose() * 180.0 / Nav::PI << std::endl;

    // Eigen::Matrix3d Q;
    // Q << 1, 2, 3,
    //         2, 1, 3,
    //         4, 1, 2;
    // // Q << 1, 0, 0;
    // // Q << 2, 0, 0;
    // // Q << 3, 2, 1;
    // std::cout << Q << std::endl;

}

